#2024年9月27日旋转测试
#
import traceback

import matplotlib.pyplot as plt
import numpy as np

import math
import time

from Common.ControlEnum import forcemode
from Hardware.UrRobot import UrRobot
from Hardware import FT300
from Common import INIFile
from Hardware import FTur5
from Algorithm.AlgorithmModel import ControlResult
from Algorithm.M_my_test import my_test
from scipy.spatial.transform import Rotation as R
import numpy as np

def Move2Init(ur5):
    a2r = math.pi / 180
    initpose = [0 * a2r, -90 * a2r, -90 * a2r, -90 * a2r, 90 * a2r, 90 * a2r]
    ur5.moveJ(initpose, 2, 0.2)
    ur5.getActualTCPPose()

def Move(ur5,pose):
    a2r = math.pi / 180
    ur5.moveJ([x * a2r for x in pose], 2, 0.2)
    cur_pose = ur5.getActualTCPPose()
    cur_roate = cur_pose[3:]
    rotation = R.from_rotvec(cur_roate)
    quat=rotation.as_quat()
    print("rotation x",2*rotation.as_quat()[0])

    euler_angles = rotation.as_euler('xyz', degrees=True)
    euler_angles=np.round(euler_angles,2)
    print(f"X:{euler_angles[0]},Y:{euler_angles[1]},Z:{euler_angles[2]}")
    rotation_matrix = rotation.as_matrix()
    #print(rotation_matrix)
    return rotation

def MovePose(ur5,pose):
    cur_pose = ur5.getActualTCPPose()
    cur_pose[3:]=pose
    ur5.moveL(cur_pose,0.1,0.1)

def cal(ur5,x,y,z):
    """
    传入xyz方向的增量，以及当前的姿态，计算目标姿态
    Args:
        ur5:
        x:
        y:
        z:

    Returns:

    """
    cur_pose = ur5.getActualTCPPose()
    cur_roate = cur_pose[3:]
    print(f'当前旋转矢量{cur_roate}')

    # 将旋转角度转换为旋转对象（单位为弧度，需用np.radians转换）
    rotation_x = R.from_euler('x', np.radians(x), degrees=False)
    rotation_y = R.from_euler('y', np.radians(y), degrees=False)
    rotation_z = R.from_euler('z', np.radians(z), degrees=False)
    target_rotation = rotation_z * rotation_y * rotation_x#组合旋转：Z -> Y -> X
    #target_rotation = rotation_x * rotation_y * rotation_z#组合旋转：Z -> Y -> X

    cal_eular(target_rotation)#验证一下结果
    #target_rotation = rotation_x * rotation_y * rotation_z
    current_rotation = R.from_rotvec(cur_roate)#计算当前的旋转矩阵
    #new_rotation = current_rotation * target_rotation#右乘
    new_rotation = target_rotation*current_rotation#左乘得到新的坐标
    new_rotation_vector = new_rotation.as_rotvec()#新姿态转化为旋转矢量，并返回

    ###打印验证可不用
    cal_eular_delta(new_rotation,current_rotation.inv())
    cur_euler=current_rotation.as_euler('xyz', degrees=True)
    euler_angles = new_rotation.as_euler('xyz', degrees=True)
    print(f'cureuler:{cur_euler},euler:{euler_angles}')
    ###
    return new_rotation_vector

def cal_eular_delta(R1,R2):
    R0=R1*R2
    R0=R0.as_euler('xyz', degrees=True)
    R0=np.round(R0,2)
    print(f'欧拉角差值{R0}')
    return R0

def cal_eular(R):
    R0=R.as_euler('xyz', degrees=True)
    print(f'欧拉角差值{R0}')

def cal_eular_delta_vec(V1,V2):
    """
    delta = V1*V2.inv()#左乘,相当于V1-V2。
    Args:
        V1: 目标旋转向量
        V2: 当前旋转向量

    Returns:

    """
    R1=R.from_rotvec(V1)
    R2=R.from_rotvec(V2)
    eular=cal_eular_delta(R1,R2.inv())
    return eular

def main():
    # region 1,初始化机器人、力传感器
    # region 获取配置文件
    a2r = math.pi / 180
    config = INIFile.IniConfig('config.ini')
    robotip = config.read('hardware', 'robotip')
    useFt = config.read('hardware', 'UseForceSensor')
    robot_type = config.read('hardware', 'robot')
    teachMode = forcemode(int(config.read('hardware', 'Teachmode')))
    dt = 0.008 if robot_type == "ur5" else 0.002
    print(robotip, useFt, dt, teachMode)
    robotip="192.168.199.128"
    ur5 = UrRobot(robotip=robotip, forcesensor=FTur5.Ftur5(), dt=dt)
    ur5.init()
    # endregion

    Move2Init(ur5)
    #Move(ur5, [0 , -90, -90, -90 , 90 , 90])
    #Move(ur5,[0 , -90 , -90 , -90 , 90 , 0 ])
    # Move(ur5,[0,-77,-107,-65,90,0])
    #Move(ur5, [4.05, -78.73, -105.75, -69.97, 83.1, 23.03])

    initpose=ur5.getActualTCPPose()[3:]
    # r=cal(ur5,30,0,0)
    # MovePose(ur5, r)
    #
    # r=cal(ur5,0,0,90)
    # MovePose(ur5, r)
    # Move2Init(ur5)
    ####这个是一次性旋转
    r=cal(ur5,30,30,90)#等效于单独绕xyz 30-》0-》90，注意不能反过来
    MovePose(ur5, r)

    # r=cal(ur5,10,0,0)#等效于单独绕xyz 30-》0-》90，注意不能反过来
    # MovePose(ur5, r)
    #考虑转回去原始姿态
    delta=cal_eular_delta_vec(initpose,ur5.getActualTCPPose()[3:])#这个算出来不是的，是30，-30，-90
    #delta=cal_eular_delta_vec(ur5.getActualTCPPose()[3:],initpose)#这个算出来就是30，30，90
    r=cal(ur5,delta[0],delta[1],delta[2])
    MovePose(ur5, r)

    ####这个是分解成各个轴单独旋转
    r=cal(ur5,30,0,0)
    MovePose(ur5, r)
    r=cal(ur5,0,30,0)
    MovePose(ur5, r)
    r=cal(ur5,0,0,90)
    MovePose(ur5, r)

    #考虑转回去原始姿态
    delta=cal_eular_delta_vec(initpose,ur5.getActualTCPPose()[3:])
    #delta=cal_eular_delta_vec(ur5.getActualTCPPose()[3:],initpose)
    r=cal(ur5,delta[0],delta[1],delta[2])
    MovePose(ur5, r)

    ur5.disconnect()

if __name__ == '__main__':
    main()



